package clubhouse.motioncommand;

import clubhouse.base.Base;

public class CalculInfo {
	private Integer pulse_dist;
	private Integer pulse_rot;
	private Integer trigo;
	
	public double delta;	//pulses per rad
	public double omega;	//pulses per unit
	
	public Integer getPulse_dist() {
		return pulse_dist;
	}

	public Integer getPulse_rot() {
		return pulse_rot;
	}

	public Integer getTrigo() {
		return trigo;
	}
	
	private double euclideanDist(double x1, double y1, double x2, double y2){
		return(Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)));
	}

	public CalculInfo() {
		this.delta = 50;
		this.omega = 100/Math.PI;
	}

	
	public void calculate_move_info(double x_robot, double y_robot, double r_robot, double[] coord_dest){
		double rotation,distance;
		double offset = 0,coefDir=0;
		//Pulse Rotation
		coefDir = (coord_dest[1]-y_robot)/(coord_dest[0]-x_robot);
		if (coord_dest[0] - x_robot < 0){
			offset = -Math.PI;
		}
		rotation = Math.atan(coefDir) - r_robot + offset;
		if (rotation <= -Math.PI) rotation = rotation + 2*Math.PI;
		if (rotation > Math.PI) rotation = rotation - 2*Math.PI;
		if (Base.DEBUG) System.out.println("(calculate_move_info) coefDir = "+coefDir+", atan = "+Math.atan(coefDir)+" rd, r_robot = "+r_robot+", offset = "+offset);
		this.pulse_rot = (int) Math.round(Math.abs(rotation*this.omega));
		if (Base.DEBUG) System.out.println("(calculate_move_info) rotation = "+rotation+" rd, pulses = "+this.pulse_rot);
			
		//Trigo rotation
		if (rotation >= 0){
			this.trigo = 1;
		} else {
			this.trigo = 0;
		}
		if (Base.DEBUG) System.out.println("(calculate_move_info) trigo = "+this.trigo);
		
		//Pulse Distance
		distance = euclideanDist(x_robot, y_robot, coord_dest[0], coord_dest[1]);
		if (distance > 1.5){
			distance = 1;
		}
		this.pulse_dist = (int) Math.round(distance * this.delta);
		if (Base.DEBUG) System.out.println("(calculate_move_info) distance = "+distance+" unit(s), pulses = "+this.pulse_dist);
		
		
	}
}
